/**
 * @FilePath: \ros2\src\agv_perception\two_dimensional_code\qrodom\src\qrodom.cpp
 * @Date: 2025-03-18 11:47:29
 * @Description:  
 * @Author: kongyucai yucai.kong@lgmgim.cn
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-22 14:31:20
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
/******************************************/

#include "qrodom/qrodom.hpp"
#include <fstream>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <shared_parameters/shared_params_interface.hpp>

#define rad2deg 180.0 / M_PI
#define INIT_POX_X -9999
#define INIT_POX_Y -9999
#define INIT_POX_A 0
// #define PACKAGE_NAME "agv_navmap"            //  mapTest.xml在agv_navmap包
#define LANDMARK__FILE_NAME "/map/mapTest.xml"
// 2023.5.23
#define INITPOSE_FILE_NAME "/map/qr_init_pose.yaml"
#define EVENTNONE      0
#define EVENTWARNING   1
#define EVENTERROR     2
#define QRNAVIGATION   1 

using namespace tinyxml2;
using std::placeholders::_1;
using namespace std::chrono_literals;

const uint8_t AGVModeManual = agv_msgs::msg::AGVMode::MANUAL;
const uint8_t AGVModeAuto = agv_msgs::msg::AGVMode::AUTOMATIC;
const uint8_t AGVModeLocal = agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC;
const std::string default_floor = "1_Floor";

std::mutex mtx_, mutexA_, mutexB_, mutexC_;
double delta_time = 0, delta_x_odom = 0, delta_y_odom = 0, delta_th_odom = 0;

bool qrCode_exist, qrCode_find, qrCode_finded, first_in = false, first_in_2 = false, first_in_3 = false, origin_in = false, target_find = false;

namespace xml_tool
{
  std::string FindMapId(XMLElement *root)
  {
    DEBUG_STREAM_OUT("FindMapId start");
    if (!root)
    {
      DEBUG_ERROR_OUT("FindMapId failed: root is null");
      return "";
    }
    try
    {
      std::string robot_id = std::string(getenv("ROBOT_ID"));
      std::string group_name = "group";
      std::string agv_number_name = "agv_number";
      XMLElement *first_element = root->FirstChildElement(group_name.c_str());

      while (first_element != NULL)
      {
        std::string cur_element_name = std::string(first_element->Name());
        if (group_name == cur_element_name)
        {
          std::string map_id = first_element->Attribute("mapId") ? first_element->Attribute("mapId") : "";
          XMLElement *first_agv_number = first_element->FirstChildElement(agv_number_name.c_str());
          while (first_agv_number != NULL)
          {
            std::string cur_number_name = std::string(first_agv_number->Name());
            if (agv_number_name == cur_number_name)
            {
              std::string id = first_agv_number->Attribute("id") ? first_agv_number->Attribute("id") : "null_agv_id";
              if (id == robot_id)
              {
                DEBUG_STREAM_OUT("find cur map id ok mapId:" << map_id << ",cur_robot_id:" << robot_id);
                return map_id;
              }
            } // 第二层

            first_agv_number = first_agv_number->NextSiblingElement();
          }
        } // 第一层
        first_element = first_element->NextSiblingElement();
      }
    }
    catch (...)
    {
      DEBUG_ERROR_OUT("error: FindMapId crash");
    }

    return "map_id_null";
  }

  XMLElement *FindMapData(XMLElement *root, const std::string &target_map_id)
  {
    DEBUG_STREAM_OUT("FindMapData start");
    if (!root)
    {
      DEBUG_ERROR_OUT("FindMapData failed: root is null");
      return NULL;
    }

    try
    {
      std::string map_name = "map";
      XMLElement *first_element = root->FirstChildElement(map_name.c_str());

      while (first_element != NULL)
      {
        std::string cur_element_name = std::string(first_element->Name());
        if (map_name == cur_element_name)
        {
          std::string map_id = first_element->Attribute("id") ? first_element->Attribute("id") : "";
          if (target_map_id == map_id)
          {
            DEBUG_STREAM_OUT("find target_map_id ok:target_map_id:" << target_map_id);
            return first_element;
          }
        }
        first_element = first_element->NextSiblingElement();
      }
    }
    catch (...)
    {
      DEBUG_ERROR_OUT("error: FindMapData crash");
    }

    return NULL;
  }
} // End namespace

Qrodom::Qrodom() : Node("qrodom_node"), 
    kalmanfilter_(), 
    tf_odom2map_(), 
    tf_base2odom_(),
    current_odometry_(),
    last_pose_msg_(), 
    pose_msg_(),
    current_pose_(), 
    target_pose_(), 
    origin_pose_(),
    pose_file_(""), 
    c1(""), 
    c2(""), 
    current_floor_(default_floor), 
    current_tag_(0), 
    origin_tag_(0), 
    target_tag_(0),
    last_tag_(0), 
    tag_count_(0), 
    tag_count_num_(0), 
    qr_count_(0),
    to_node_(0), 
    from_node_(0),
    use_odom_shift_(false), 
    qr_initialized_(false), 
    parameters_state_(false),
    origin_init_flag_(false), 
    agv_mode_flag_(false), 
    has_odometry_(false), 
    qr_init_flag_(false),
    current_mode_(0), 
    last_mode_(0),
    agv_navigation_type_(0),
    qr_code_miss_count_(0), 
    qr_code_miss_threshold_(5), 
    filter_switch_(0),
    qr_code_error_state_(0),
    init_trans_x_(0.0f), 
    init_trans_y_(0.0f), 
    init_trans_angle_(0.0f),
    qr_code_miss_distance_(0.0), 
    filter_scale_(1.0),
    missed_tags_(),
    last_update_time_(this->now()), 
    pose_yaw_(0.0)
{
  init_parameters();
  initialize_components();
  setup_communications();
  start_timers();
}

Qrodom::~Qrodom() 
{
}

void Qrodom::initialize_components() 
{
  tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
  
  tf_odom2map_ = Transform::Identity();
  tf_base2odom_ = Transform::Identity();
  qrmap_init();
  init_pose();
}

void Qrodom::setup_communications() 
{
  update_service_ = this->create_service<agv_srvs::srv::UpdateQr>(
      "update_qr",
      std::bind(&Qrodom::handle_update_request, this, _1, std::placeholders::_2));

  odom_sub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
      "/odom", 10,
      std::bind(&Qrodom::handle_odometry, this, _1));

  qr_link_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
      "/landmark", 10,
      std::bind(&Qrodom::handle_qr_link, this, _1));

  pose_sub_ = this->create_subscription<agv_msgs::msg::PoseWithConfidence>(
      "/pose", 10,
      std::bind(&Qrodom::handle_pose, this, _1));

  move_info_sub_ = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
    "/auto_run_status", 1, 
    std::bind(&Qrodom::handle_move_info, this, _1));

  agv_mode_sub_ = this->create_subscription<agv_msgs::msg::AGVMode>(
    "/agvmode", 1, 
    std::bind(&Qrodom::handle_mode_info, this, _1));

  floor_sub_ = this->create_subscription<agv_msgs::msg::InitPose>(
    "/InitPose", 1, 
    std::bind(&Qrodom::handle_floor_message, this, _1));

  pose_pub_ = this->create_publisher<agv_msgs::msg::PoseWithConfidence>("/pose", 10);
  event_pub_ = this->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 10);
}

void Qrodom::start_timers() 
{
  timers_.push_back(this->create_wall_timer(100ms, std::bind(&Qrodom::record_pose, this)));
  timers_.push_back(this->create_wall_timer(200ms, std::bind(&Qrodom::init_map, this)));
  timers_.push_back(this->create_wall_timer(1s, std::bind(&Qrodom::publish_error, this)));
  timers_.push_back(this->create_wall_timer(25ms, std::bind(&Qrodom::calculate_position, this)));
}

void Qrodom::qrmap_init() 
{
  XMLDocument doc;
  std::string qr_xml_file = std::string(getenv("HOME")) + LANDMARK__FILE_NAME; // xml路径

  if (doc.LoadFile(qr_xml_file.c_str()) != 0) // 加载xml
  {
    DEBUG_ERROR_OUT("load xml file failed " << qr_xml_file);
    return;
  }

  XMLElement *root = doc.RootElement(); // xml根节点
  if (!root)
  {
    DEBUG_ERROR_OUT("xml root is nullptr");
    return;
  }
  // add at 20230701 by yxj
  // 根据小车所属的地图；加载制定地图数据
  std::string mapId = xml_tool::FindMapId(root);
  XMLElement *secondRoot = xml_tool::FindMapData(root, mapId);
  if (!secondRoot)
  {
    DEBUG_ERROR_OUT("xml secondRoot is nullptr");
    return;
  }
  //
  std::string point_element_name = "point";
  XMLElement *point_Node = secondRoot->FirstChildElement(point_element_name.c_str()); // xml中, "point"子节点
  floor_info_.clear(); // 重新加载一次xml会出现旧数据还存在的bug
  // 2023.3.13
  while (point_Node != NULL) // 获取"point"子节点属性, 即二维码属性
  {
    try
    {
      int qrCodeId = point_Node->IntAttribute("qrCodeId"); // ID
      // string zonetag_ = point_Node->Attribute("zonetag");
      const char *attrValue = point_Node->Attribute("zoneTag");
      if (qrCodeId > 0 && attrValue != nullptr) {

        std::string zoneTag_ = point_Node->Attribute("zoneTag");
        int id = point_Node->IntAttribute("id");
        float x = point_Node->FloatAttribute("x");
        float y = point_Node->FloatAttribute("y");
        float yaw = point_Node->FloatAttribute("qrAngle"); // 二维码坐标系相对map坐标系夹角, 即偏航角(绕z轴旋转)
        // 2023.5.22
        float qrOffsetX = point_Node->FloatAttribute("qrOffsetX");
        float qrOffsetY = point_Node->FloatAttribute("qrOffsetY");
        float qrOffsetAngle = point_Node->FloatAttribute("qrOffsetAngle");
        bool is_swith_point = point_Node->BoolAttribute("isSwitchPoint");
        floor_info_[zoneTag_].qr_map.emplace(qrCodeId,
          QrMapPoint(x + qrOffsetX, y + qrOffsetY, yaw + qrOffsetAngle, is_swith_point));
        
        geometry_msgs::msg::Pose pose_temp;
        pose_temp.position.x = x + qrOffsetX;
        pose_temp.position.y = y + qrOffsetY;
        Eigen::Quaterniond eigen_quat = Transform::Identity().RollPitchYaw(0, 0, yaw + qrOffsetAngle);
        pose_temp.orientation.x = eigen_quat.x();
        pose_temp.orientation.y = eigen_quat.y();
        pose_temp.orientation.z = eigen_quat.z();
        pose_temp.orientation.w = eigen_quat.w();

        floor_info_[zoneTag_].map_points.emplace(id, MapPoint(qrCodeId, pose_temp, is_swith_point));
        std::cout << "#1 floor:" << zoneTag_ << 
          ",id:" << id << 
          ",qrCodeId:" << qrCodeId << 
          ",x:" << floor_info_[zoneTag_].qr_map[qrCodeId].x << 
          ",y:" << floor_info_[zoneTag_].qr_map[qrCodeId].y << 
          ",yaw:" << floor_info_[zoneTag_].qr_map[qrCodeId].angle << std::endl;
      } else if (qrCodeId > 0 && attrValue == nullptr) {
        std::string zoneTag_ = default_floor;
        int id = point_Node->IntAttribute("id");
        float x = point_Node->FloatAttribute("x");
        float y = point_Node->FloatAttribute("y");
        float yaw = point_Node->FloatAttribute("qrAngle"); // 二维码坐标系相对map坐标系夹角, 即偏航角(绕z轴旋转)
        // 2023.5.22
        float qrOffsetX = point_Node->FloatAttribute("qrOffsetX");
        float qrOffsetY = point_Node->FloatAttribute("qrOffsetY");
        float qrOffsetAngle = point_Node->FloatAttribute("qrOffsetAngle");
        floor_info_[zoneTag_].qr_map.emplace(qrCodeId, 
          QrMapPoint(x + qrOffsetX, y + qrOffsetY, yaw + qrOffsetAngle, false));
        geometry_msgs::msg::Pose pose_temp;
        pose_temp.position.x = x + qrOffsetX;
        pose_temp.position.y = y + qrOffsetY;
        Eigen::Quaterniond eigen_quat = Transform::Identity().RollPitchYaw(0, 0, yaw + qrOffsetAngle);
        pose_temp.orientation.x = eigen_quat.x();
        pose_temp.orientation.y = eigen_quat.y();
        pose_temp.orientation.z = eigen_quat.z();
        pose_temp.orientation.w = eigen_quat.w();
        floor_info_[zoneTag_].map_points.emplace(id, MapPoint(qrCodeId, pose_temp, false));
        std::cout << "#2 floor:" << zoneTag_ << 
          ",id:" << id << 
          ",qrCodeId:" << qrCodeId << 
          ",x:" << floor_info_[zoneTag_].qr_map[qrCodeId].x << 
          ",y:" << floor_info_[zoneTag_].qr_map[qrCodeId].y << 
          ",yaw:" << floor_info_[zoneTag_].qr_map[qrCodeId].angle << std::endl;
      }
      point_Node = point_Node->NextSiblingElement(point_element_name.c_str()); // 下一子节点
    }
    catch (const std::exception &e)
    {
      DEBUG_ERROR_OUT("qrodom load map crash:" << std::string(e.what()));
    }
    catch (...)
    {
      DEBUG_ERROR_OUT("error: read xml crash ");
    }
  }
}

void Qrodom::init_pose()
{
  pose_file_ = std::string(getenv("HOME")) + INITPOSE_FILE_NAME;
  try
  {
    // cur_floor = default_floor;
    YAML::Node loaded = YAML::LoadFile(pose_file_);
    if (!loaded) {
      DEBUG_WARN_OUT("Init pose yaml load fail");
      return;
    }

    double pose_x = loaded["translation"].as<std::vector<double>>().at(0);
    double pose_y = loaded["translation"].as<std::vector<double>>().at(1);
    double pose_z = loaded["translation"].as<std::vector<double>>().at(2);

    double pose_angle = loaded["yaw"].as<std::vector<double>>().at(0);
    origin_pose_.position.x = pose_x;
    origin_pose_.position.y = pose_y;
    DEBUG_OUT("File load, pose x-%.4f, pose y-%.4f, pose angle-%.4f", pose_x, pose_y, pose_angle);

    geometry_msgs::msg::TransformStamped tf_odom2base;
    try {
      tf_odom2base = tf_buffer_->lookupTransform(
        "base_link",            // target_frame
        "odom",                 // source_frame
        tf2::TimePoint(), 
        tf2::durationFromSec(0.3)  
      );
    } catch (const tf2::TransformException& ex) {
      DEBUG_OUT_THROTTLE(*get_clock(), 1000,
        "Transform error:" << ex.what());
    }
   
    Transform base2map_transform = Transform::Identity();
    base2map_transform.setTransform(pose_x, pose_y, pose_angle);
    
    auto& t = tf_odom2base.transform.translation;
    auto& r = tf_odom2base.transform.rotation;
    Eigen::Vector3d odom2base_translation(t.x, t.y, t.z);
    Eigen::Quaterniond odom2base_rotation = Transform::Identity().ToEigen(r);
    Transform odom2base_transform(odom2base_translation, odom2base_rotation);
   
    tf_odom2map_ = base2map_transform * odom2base_transform;

    qr_init_flag_ = true;
    
    Eigen::Matrix4d mat = tf_odom2map_.matrix44();
    init_trans_x_ = mat(0, 3); 
    init_trans_y_ = mat(1, 3); 
    
    init_trans_angle_ = acosf(std::min(std::max(mat(0, 0), -1.0), 1.0));
    if (mat(1, 0) < 0)
      init_trans_angle_ = -init_trans_angle_;

    tf2::Quaternion quat_rpy;
    quat_rpy.setRPY(0.0, 0.0, init_trans_angle_);  
    quat_rpy.normalize(); 
    pose_msg_.pose.pose.orientation = tf2::toMsg(quat_rpy);

    DEBUG_OUT(
      "Init odomtrans, init x-%.4f, init y-%.4f, init angle-%.4f", 
      init_trans_x_, init_trans_y_, init_trans_angle_);
  }
  catch (const YAML::Exception &e)
  {
    DEBUG_WARN_OUT("Load:" << pose_file_ << ",error-------" << e.what());
  }
  catch (const std::exception &e)
  {
    DEBUG_ERROR_OUT("Error:" << e.what());
  }
}

void Qrodom::handle_move_info(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg)
{
  DEBUG_STREAM_OUT("HandleMoveInfoMessage start");
  from_node_ = msg->fromnode;
  to_node_ = msg->tonode;
  DEBUG_STREAM_OUT("HandleMoveInfoMessage end");
}

void Qrodom::handle_mode_info(const agv_msgs::msg::AGVMode::SharedPtr msg)
{
  DEBUG_STREAM_OUT("HandleModeInfoMessage start");
  current_mode_ = msg->mode;
}

void Qrodom::handle_floor_message(const agv_msgs::msg::InitPose::SharedPtr msg)
{
  DEBUG_OUT("HandleFloorMessage start");
  // string s = "";
  // c1 = ros::param::param("/AGVParam/current_floor",default_floor);
  DEBUG_WARN_OUT("c1" << c1 
    << " c2 " << msg->zonetag << " = " << (c1 == msg->zonetag) 
    << " msg active is " << msg->active << 
    " cur_floor is" << current_floor_);

  if (c1 == msg->zonetag && msg->active == true) {
    current_floor_ = msg->zonetag;
  }
  DEBUG_STREAM_OUT( "HandleFloorMessage end");
}

void Qrodom::handle_pose(agv_msgs::msg::PoseWithConfidence::SharedPtr msg)
{
  DEBUG_STREAM_OUT( "handle_PoseMsgs callback start");
  // std::lock_guard<std::mutex> lock(mtx);
  if (agv_navigation_type_ != QRNAVIGATION) {
    current_pose_ = msg->pose.pose;
    origin_pose_ = msg->pose.pose;
    pose_msg_ = *msg;
    try
    {
      Transform base2map_translation = Transform::Identity();
      base2map_translation.setTransform(current_pose_);
      
      tf_odom2map_ = base2map_translation * tf_base2odom_.inverse();
    }
    catch (...)
    {
      DEBUG_ERROR_OUT("tf caculate error from other nav type!!!");
    }
  }

  DEBUG_STREAM_OUT("handle_PoseMsgs callback end");
}

void Qrodom::handle_odometry(const agv_msgs::msg::OdometryLite::SharedPtr msg) 
{
  DEBUG_STREAM_OUT("handle_Odometrymsgs callback start");
  // std::lock_guard<std::mutex> lock(mtx);
  rclcpp::Time now = this->now();
  rclcpp::Duration duration = now - last_update_time_;
  DEBUG_STREAM_OUT( 
    "handle_Odometrymsgs callback calc duration=" << duration.seconds() << " s");
  double x = msg->pose.position.x;
  double y = msg->pose.position.y;
  double z = msg->pose.position.z;
  DEBUG_STREAM_OUT( "handle_Odometrymsgs callback msgToTf");
  
  tf_base2odom_.setTransform(msg->pose);
  DEBUG_STREAM_OUT("handle_Odometrymsgs callback middle");
  // ROS_WARN_STREAM("quat x"<<quat.x()<<" quat y "<<quat.y()<<" quat z"<<quat.z()<<" quat w "<<quat.w());
  
  DEBUG_WARN_OUT(
    "after normalize quat x " << tf_base2odom_.rotation().x() <<
    " quat y " << tf_base2odom_.rotation().y() << 
    " quat z " << tf_base2odom_.rotation().z() << 
    " quat w " << tf_base2odom_.rotation().w() << 
    " yaw " << tf_base2odom_.GetYaw());

  //cur_odom = msgs;
  set_current_odometry(*msg);

  DEBUG_STREAM_OUT( "handle_Odometrymsgs callback RToO");
  Transform pose_transform = tf_odom2map_ * tf_base2odom_;
  
  Eigen::Matrix4d pose_mat = pose_transform.matrix44();
  double pos_x = pose_mat(0, 3);
  double pos_y = pose_mat(1, 3);
  double pos_yaw = acosf(std::min(std::max(pose_mat(0, 0), -1.0), 1.0));
  if (pose_mat(1, 0) < 0)
    pos_yaw = -pos_yaw;
  if (!std::isnan(pos_yaw))
    set_current_yaw(pos_yaw);

  Eigen::Matrix4d odom_mat = tf_odom2map_.matrix44();
  double OtoM_x = odom_mat(0, 3);
  double OtoM_y = odom_mat(1, 3);
  //double OtoM_yaw = acosf(T_OtoM(0, 0));
  double OtoM_yaw = acosf(std::min(std::max(odom_mat(0, 0),-1.0), 1.0));
  if (odom_mat(1, 0) < 0)
    OtoM_yaw = -OtoM_yaw;

  DEBUG_STREAM_OUT( "handle_Odometrymsgs callback OtoM_yaw=" << OtoM_yaw);
  Eigen::Quaterniond quat = Transform::Identity().RollPitchYaw(0, 0, pos_yaw);
  DEBUG_STREAM_OUT( "handle_Odometrymsgs callback pos_yaw=" << pos_yaw);

  agv_navigation_type_ = SharedParamsInterface::getParam("AGVParam", "agv_navigation_type", QRNAVIGATION);
  DEBUG_STREAM_OUT( 
    "handle_Odometrymsgs callback agv_navigation_type=" << agv_navigation_type_);

  pose_msg_.pose.pose.position.x = pos_x;
  pose_msg_.pose.pose.position.y = pos_y;
  // pose_msg_.pose.pose.orientation = pose_rotation;
  pose_msg_.pose.pose.orientation.x = pose_transform.rotation().x();
  pose_msg_.pose.pose.orientation.y = pose_transform.rotation().y();
  pose_msg_.pose.pose.orientation.z = pose_transform.rotation().z();
  pose_msg_.pose.pose.orientation.w = pose_transform.rotation().w();
  pose_msg_.header.stamp = now;
  pose_msg_.zonetag = current_floor_;
  // 2023.2.17
  pose_msg_.qr_init = qr_init_flag_;

  if (qr_init_flag_) {
    pose_msg_.score = 75.0;
  }

  if (agv_navigation_type_ == QRNAVIGATION) {
    pose_pub_->publish(pose_msg_); // 发布 /pose
  }

  DEBUG_OUT("Odom = %f, %f, %f OtoM = %f, %f, %f  POS = %f, %f, %f",
           x, y, tf_base2odom_.GetYaw() * rad2deg,
           OtoM_x, OtoM_y, OtoM_yaw * rad2deg,
           pos_x, pos_y, pos_yaw * rad2deg);

  has_odometry_ = true;
  if (current_tag_ != last_tag_)
    tag_count_ = 0;
  last_update_time_ = now;
  DEBUG_STREAM_OUT("received odom msg take " << duration.seconds() << " s");
  DEBUG_STREAM_OUT("agv nav type is " << agv_navigation_type_);
  DEBUG_STREAM_OUT("handle_Odometrymsgs callback end");
}

void Qrodom::handle_qr_link(const geometry_msgs::msg::PoseStamped::SharedPtr msg) 
{
  DEBUG_OUT("get_qr_link start");

  std::lock_guard<std::mutex> lock(mtx_);
  if (!has_odometry_) {
      DEBUG_OUT("wait for (odom) msgs");
      return;
  }

  const auto now = this->now();
  const double delta_time = (now - msg->header.stamp).seconds();
  const auto cur_odom = get_current_odometry();
  const double pos_yaw = get_current_yaw();

  DEBUG_WARN_OUT("use_odom_shift " << use_odom_shift_
    << " pos_yaw " << pos_yaw
    << " cur_odom.twist.linear.x " << cur_odom.twist.linear.x
    << " cur_odom.twist.linear.y " << cur_odom.twist.linear.y
    << " cur_odom.twist.angular.z " << cur_odom.twist.angular.z
    << " delta time " << delta_time);

  if (use_odom_shift_ && 
    !std::isnan(pos_yaw) && delta_time > 0 && 
    (abs(cur_odom.twist.linear.x) > 0.01 || 
    abs(cur_odom.twist.linear.y) > 0.01 || 
    abs(cur_odom.twist.angular.z) > 0.017))
  {
    // if(use_odom_shift && !std::isnan(pos_yaw) && (abs(cur_odom.twist.linear.x)>0.01 || abs(cur_odom.twist.linear.y)>0.01 || abs(cur_odom.twist.angular.z) >0.003)){
    delta_x_odom = (cur_odom.twist.linear.x * cos(pos_yaw) - cur_odom.twist.linear.y * sin(pos_yaw)) * delta_time;
    delta_y_odom = (cur_odom.twist.linear.x * sin(pos_yaw) + cur_odom.twist.linear.y * cos(pos_yaw)) * delta_time;
    delta_th_odom = cur_odom.twist.angular.z * delta_time;
    DEBUG_WARN_OUT("delta_x_odom " << delta_x_odom
                << " delta_y_odom " << delta_y_odom
                << " delta_th_odom " << delta_th_odom
                << " delta time " << delta_time);
  } else {
    delta_x_odom = 0;
    delta_y_odom = 0;
    delta_th_odom = 0;
  }

  if (std::isnan(pos_yaw)) {
    DEBUG_ERROR_OUT("pose_yaw is nan!!!");
  }

  // landmark的回调, 话题在扫到二维码才有数据, 才会进入回调
  // 2023.2.17 二维码初始化标志
  if (qr_count_ < 1)
    qr_count_++;
  else if (!qr_init_flag_)
    qr_init_flag_ = true;

  // 1.计算th_dm, 通过landmark话题数据(dm->base), 和xml保存二维码坐标(qr->map), 矩阵运算得到(base->map)变换矩阵
  current_tag_ = msg->pose.position.z; // dm id
  // DEBUG_STREAM_OUT("id = %d", TAG);
  std::map<uint32_t, QrMapPoint>::iterator it = floor_info_[current_floor_].qr_map.find(current_tag_); // 通过id找保存xml数据map
  DEBUG_STREAM_OUT( "get_qr_link:TAG=" << current_tag_ << ",current_floor:" << current_floor_);
  if (it != floor_info_[current_floor_].qr_map.end()) {
    // #1 base_link 在 Qr tf
    Transform base2dm_transform = Transform::Identity();
    base2dm_transform.setTransform(msg->pose);                                        // base to dm tranfrom
    DEBUG_STREAM_OUT("get_qr_link:find start");

    // #2 Qr 在Map中的tf
    QrMapPoint &qrPoint_temp = it->second;                                            // xml, 二维码坐标
    Transform dm2map_transform = Transform::Identity();
    dm2map_transform.setTransform(qrPoint_temp.x, qrPoint_temp.y, qrPoint_temp.angle); // dm to map transform
    // DEBUG_STREAM_OUT( "dm2map_transform:\n" << dm2map_transform.matrix44());

    // #3 得出base_link 在 map 的 tf
    // 矩阵运算得到, base_link->map, 即机器人位姿，从bask_link到QRcode的tf算得
    Transform base2map_transform = dm2map_transform * base2dm_transform;              // base to map transform

    //float th_dm = acosf(T_RtoM(0, 0));
    Eigen::Matrix4d mat = base2map_transform.matrix44();
    double x = mat(0, 3);
    double y = mat(1, 3);
    float th_dm = acosf(std::min(std::max(mat(0, 0), -1.0), 1.0));
    if (mat(1, 0) < 0)
      th_dm = -th_dm; // 机器人在地图中的方向, 二维码读头数据计算得

    Transform last_pose_transform = Transform::Identity();
    last_pose_transform.setTransform(pose_msg_.pose.pose);
    double last_pose_yaw = last_pose_transform.GetYaw();
    float fNewYawTemp = th_dm;
    DEBUG_STREAM_OUT("get_qr_link:find middle 2 fNewYawTemp=" << fNewYawTemp);

    if (abs(th_dm - last_pose_yaw) <= 1 && qr_init_flag_ && filter_switch_ == 0) {             // 判断差值是否小于1弧度,滤波不开启时默认用该方法
      fNewYawTemp = filter_scale_ * th_dm + (1 - filter_scale_) * last_pose_yaw;               // 可注释, 只使用th_dm
    } else if (abs(current_odometry_.twist.linear.x) > 0.5 && filter_switch_ == 1) {
      // thetaMeasurement = fNewYawTemp;
      DEBUG_WARN_OUT("fNewYawTempOld is " << fNewYawTemp);
      fNewYawTemp = kalmanfilter_.update(fNewYawTemp);
      DEBUG_WARN_OUT("fNewYawTempNew is " << fNewYawTemp << " TAG is " << current_tag_);
    }

    if (use_odom_shift_)
    {
      base2map_transform.setTransform(x + delta_x_odom, y + delta_y_odom, fNewYawTemp);
      DEBUG_OUT("get_qr_link:find use_odom_shift");
    }
    else
    {
      base2map_transform.setTransform(x, y, fNewYawTemp);
      DEBUG_OUT("get_qr_link:find not use_odom_shift");
    }
    // #6 odom在Map矩阵 = 修正的base_link在Map矩阵 ×  odom 在base_link的矩阵
    // 生成Odom在map矩阵，然后广播出去此tf信息
    //T_OtoM = T_newRtoM * T_RtoO.inverse(); // base_link->map, odom->base_link, 矩阵运算得到odom->map
    tf_odom2map_ = base2map_transform * tf_base2odom_.inverse();

    Eigen::Matrix4d base2odom = tf_base2odom_.matrix44();
    double x_RtoO = base2odom(0, 3);
    double y_RtoO = base2odom(1, 3);
    //double r_RtoO = acosf(T_RtoO(0, 0));
    double r_RtoO = acosf(std::min(std::max(base2odom(0, 0), -1.0), 1.0));
    if (base2odom(1, 0) < 0)
      r_RtoO = -r_RtoO;

    Eigen::Matrix4d odom2map = tf_odom2map_.matrix44();
    double x_OtoM = odom2map(0, 3);
    double y_OtoM = odom2map(1, 3);
    //double r_OtoM = acosf(T_OtoM(0, 0));
    double r_OtoM = acosf(std::min(std::max(odom2map(0, 0), -1.0), 1.0));
    if (odom2map(1, 0) < 0)
      r_OtoM = -r_OtoM;

    double landmark_yaw = base2dm_transform.GetYaw();
    DEBUG_OUT("landmark = (%d, %f, %f, %f) Odom = (%f, %f, %f) OtoM = (%f, %f, %f) last POS = (%lf, %lf, %lf) POS = (%f, %f, %f, %lf)",
             current_tag_, msg->pose.position.x, msg->pose.position.y, landmark_yaw * rad2deg,
             x_RtoO, y_RtoO, r_RtoO * rad2deg,
             x_OtoM, y_OtoM, r_OtoM * rad2deg,
             pose_msg_.pose.pose.position.x, pose_msg_.pose.pose.position.y, last_pose_yaw * rad2deg,
             x, y, th_dm * rad2deg, fNewYawTemp * rad2deg);
    last_tag_ = current_tag_;
    tag_count_++;
  } else {
    DEBUG_ERROR_OUT("can not find this tag in qrmap!");
  }
  DEBUG_STREAM_OUT("get_qr_link end");
}

void Qrodom::handle_update_request(
    const std::shared_ptr<agv_srvs::srv::UpdateQr::Request> request,
    std::shared_ptr<agv_srvs::srv::UpdateQr::Response> response) 
{
  if (request->trigger) {
    qrmap_init();
    response->status = true;
    DEBUG_STREAM_OUT("QR map updated successfully");
  } else {
    response->status = false;
  }
}

void Qrodom::publish_tf_transform() 
{
  geometry_msgs::msg::TransformStamped transform;
  transform.header.stamp = now();
  transform.header.frame_id = "map";
  transform.child_frame_id = "base_link";
  
  // Eigen::Affine3d current_transform(tf_odom_to_map_ * tf_base_to_odom_);
  // transform.transform = tf2::toMsg(current_transform);
  
  tf_broadcaster_->sendTransform(transform);
}

void Qrodom::record_pose() 
{
  const double POSITION_THRESHOLD = 0.01;
  const double ORIENTATION_THRESHOLD = 0.03484;

  if (agv_navigation_type_ == QRNAVIGATION) {
    auto cur_pose_copy = pose_msg_;
    Transform pose_transform = Transform::Identity();
    pose_transform.setTransform(cur_pose_copy.pose.pose);
    Transform last_pose_transform = Transform::Identity();
    last_pose_transform.setTransform(last_pose_msg_.pose.pose);
    double dx = cur_pose_copy.pose.pose.position.x - last_pose_msg_.pose.pose.position.x;
    double dy = cur_pose_copy.pose.pose.position.y - last_pose_msg_.pose.pose.position.y;
    double dYaw = pose_transform.GetYaw() - last_pose_transform.GetYaw();

    if (dx > POSITION_THRESHOLD || dy > POSITION_THRESHOLD || std::abs(dYaw) >= ORIENTATION_THRESHOLD) {
      DEBUG_STREAM_OUT("Recording new pose...");

      std::map<std::string, std::vector<double>> data = {
          {"translation", {cur_pose_copy.pose.pose.position.x, cur_pose_copy.pose.pose.position.y, cur_pose_copy.pose.pose.position.z}},
          {"yaw", {pose_transform.GetYaw()}}
      };

      std::map<std::string, std::string> floor_record = {{"floor", current_floor_}};

      try {
        DEBUG_STREAM_OUT("RecordPose write file start");
        YAML::Emitter emitter;
        emitter << YAML::BeginMap;

        for (const auto &pair : data) {
          emitter << YAML::Key << pair.first << YAML::Value << YAML::BeginSeq; 
          for(const auto &element:pair.second) {
            emitter << element;
          }
          emitter << YAML::EndSeq;
        }

        for (const auto &pair : floor_record) {
          emitter << YAML::Key << pair.first << YAML::Value << pair.second << YAML::EndSeq;
        }

        emitter << YAML::EndMap;

        std::ofstream file(pose_file_);
        if (!file.is_open()) {
            // ROS_ERROR("Failed to open file for writing.");
            return;
        }

        file << emitter.c_str();
        file.close();

        DEBUG_STREAM_OUT("Poses successfully written to file.");
      } catch (const YAML:: Exception &e) {
        DEBUG_ERROR_OUT("Error while writing poses to file:"<< e.what());
      }

      last_pose_msg_ = pose_msg_; // Update the old pose
    }
  }

  DEBUG_STREAM_OUT("Recorded Pose completed.");
}

void Qrodom::init_map()
{
  DEBUG_STREAM_OUT("InitMap start");
  const std::regex pattern(".*Floor");
  c1 = SharedParamsInterface::getParam<std::string>("AGVParam", "current_floor", default_floor);
  
  DEBUG_OUT("c1 is %s", c1.c_str());
  if (!std::regex_search(c1, pattern)) {
    c1 = default_floor;
  }
  
  if (current_mode_ == AGVModeManual) {
    current_floor_ = c1;
  }

  if ((current_mode_ == AGVModeManual && last_mode_ >= AGVModeLocal) || 
    (current_mode_ >= AGVModeLocal && last_mode_ == AGVModeManual)) {
    current_floor_ = SharedParamsInterface::getParam<std::string>("AGVParam", "current_floor", default_floor);
    if (!std::regex_search(current_floor_, pattern)) {
      current_floor_ = default_floor;
    }
  }

  if (current_mode_ >= AGVModeLocal && last_mode_ == AGVModeManual) {
    get_qrodom_paramters();
    // qrmap_Init(); // 初始化二维码地图
  }
  
  last_mode_ = current_mode_;
  DEBUG_STREAM_OUT("InitMap end");
}

void Qrodom::publish_error()
{
  DEBUG_OUT("Publishing error status for floor: %s", current_floor_.c_str());
  auto event_msg = agv_msgs::msg::AGVEvent();

  event_msg.header.stamp = now();
  event_msg.header.frame_id = get_name();

  event_msg.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
  event_msg.type = agv_msgs::msg::AGVEvent::INFO;
  event_msg.action = agv_msgs::msg::AGVEvent::NONE;

  {
    std::lock_guard<std::mutex> lock(data_mutex_);
    if (qr_code_miss_count_ < qr_code_miss_threshold_) {
      qr_code_error_state_ = EVENTNONE;
    } else if (qr_code_error_state_ == 0) {
      qr_code_error_state_ = EVENTWARNING;
    }

    if (qr_code_error_state_ != 0 && !missed_tags_.empty()) {
      std::stringstream ss;
      for (const auto& tag : missed_tags_) {
        ss << tag << " ";
      }
      
      switch (qr_code_error_state_) {
        case EVENTWARNING:  
          event_msg.description = ss.str() + "QR code miss warning!";
          event_msg.type = agv_msgs::msg::AGVEvent::WARNING;
          event_msg.ackreq = true;
          break;

        case EVENTERROR:  
          event_msg.description = ss.str() + "QR code miss error! Please check!";
          event_msg.ecode = agv_msgs::msg::AGVEvent::QRCODE_MISS_ERROR;
          event_msg.type = agv_msgs::msg::AGVEvent::ALARM;
          event_msg.action = agv_msgs::msg::AGVEvent::STOP;
          event_msg.ackreq = true;
          break;
      }
    }
  }
  event_pub_->publish(event_msg);
  // DEBUG_STREAM_OUT("Error event published");
}

void Qrodom::calculate_position()
{
  DEBUG_STREAM_OUT("PosCal start");
  // DEBUG_STREAM_OUT("nav type:" << agv_navigation_type_ << ",mode:" << current_mode_);
  if (agv_navigation_type_ == QRNAVIGATION) {

    if (current_mode_ >= AGVModeLocal && 
      to_node_ != 65535 && 
      to_node_ < 1000000) {

      DEBUG_STREAM_OUT("PosCal qrNav");
      auto it_ = floor_info_[current_floor_].map_points.find(from_node_);
      if (it_ != floor_info_[current_floor_].map_points.end()) {
        origin_tag_ = it_->second.qr_code_id;
        if (!origin_init_flag_) {
          origin_init_flag_ = true;
        }
      } else {
        DEBUG_ERROR_OUT("can not find origin node in qrCode map!");
        return;
      }

      if ((abs(target_pose_.position.x) < 0.001 && 
          abs(target_pose_.position.y) < 0.001 && 
          !first_in_3) || 
          (current_mode_ >= AGVModeLocal && !agv_mode_flag_)) 
      {
        DEBUG_OUT("toNode is %d", to_node_);
        auto it = floor_info_[current_floor_].map_points.find(to_node_);
        if (it != floor_info_[current_floor_].map_points.end()) {
          target_pose_ = it->second.pose;
        } else {
          DEBUG_OUT_THROTTLE(*this->get_clock(), 1000, "can not find this node in qrCode map 1!");
          return;
        }
        first_in_3 = true;
        agv_mode_flag_ = true;
      }

      double target_distance = calculate_distance(pose_msg_.pose.pose, target_pose_);
      if (first_in_3 && target_distance < 0.2 && !first_in) 
      {
        qrCode_exist = true;
        first_in = true;
      } else if (first_in_3 && target_distance > 0.3 && first_in) {
        DEBUG_STREAM_OUT("PosCal qrNav first_in_3");

        if (!qrCode_find) {
          missed_tags_.insert(origin_tag_);
          qr_code_miss_count_++;
        }
        qrCode_find = false;
        qrCode_exist = false;
        first_in = false;
        first_in_2 = false;
        target_find = false;

        auto it = floor_info_[current_floor_].map_points.find(to_node_);
        if (it != floor_info_[current_floor_].map_points.end()) {
          target_pose_ = it->second.pose;
        } else {
          DEBUG_ERROR_OUT("can not find this node in qrCode map 2!");
          return;
        }
      }

      auto it = floor_info_[current_floor_].map_points.find(to_node_);
      if (it != floor_info_[current_floor_].map_points.end()) {
        target_tag_ = it->second.qr_code_id;
        
        if (current_tag_ == target_tag_) {
          qrCode_find = true;
          origin_init_flag_ = false;
          origin_pose_ = pose_msg_.pose.pose;
          missed_tags_.clear();
          qr_code_error_state_ = EVENTNONE;
          target_find = true;
          DEBUG_STREAM_OUT("origin pose update!!!");
        } else if (current_tag_ == origin_tag_ && 
                target_distance < 0.2 && 
                !target_find) {
          qrCode_find = true;
          origin_pose_ = it_->second.pose;
          missed_tags_.clear();
          qr_code_error_state_ = EVENTNONE;
        }
        
        DEBUG_OUT("pose dis: %.2f, TAG: %d, target TAG: %d, origin tag: %d",
              target_distance, current_tag_, target_tag_, origin_tag_);
      } else {
        DEBUG_ERROR_OUT("can not find this node in qrCode map 3!");
        return;
      }

      if (target_distance > qr_code_miss_distance_) {
        DEBUG_OUT("cur_pose (%.2f, %.2f) origin_pose_ (%.2f, %.2f) CUR_TAG %d target %d",
              pose_msg_.pose.pose.position.x, pose_msg_.pose.pose.position.y,
              origin_pose_.position.x, origin_pose_.position.y,
              current_tag_, target_tag_);
        qr_code_error_state_ = EVENTERROR;
      }
    } else if (current_mode_ == AGVModeManual) {
      qrCode_find = false;
      qrCode_exist = false;
      first_in = false;
      first_in_2 = false;
      first_in_3 = false;
      qr_code_miss_count_ = 0;
      agv_mode_flag_ = false;
      origin_init_flag_ = false;
      qr_code_error_state_ = EVENTNONE;
      origin_pose_ = pose_msg_.pose.pose;
      missed_tags_.clear();
    }
    
    DEBUG_OUT("cur_pose (%.2f, %.2f) origin (%.2f, %.2f) CUR_TAG %d target %d WARN!",
               pose_msg_.pose.pose.position.x, pose_msg_.pose.pose.position.y,
               origin_pose_.position.x, origin_pose_.position.y,
               current_tag_, target_tag_);
  }

  DEBUG_STREAM_OUT("PosCal end");
}

void Qrodom::get_qrodom_paramters()
{
  static bool first_init = true;
  qr_code_miss_threshold_ = SharedParamsInterface::getParam<int>("code_read_camera", "code_miss_num", 1);
  qr_code_miss_distance_ = SharedParamsInterface::getParam<double>("code_read_camera", "code_miss_distance", 2.0);
  tag_count_num_ = SharedParamsInterface::getParam<int>("code_read_camera", "tag_count_num", 5);
  filter_switch_ = SharedParamsInterface::getParam<int>("code_read_camera", "filter_switch", 0);
  filter_scale_ = SharedParamsInterface::getParam<double>("code_read_camera", "scale", 0.25);
  use_odom_shift_ = SharedParamsInterface::getParam<bool>("code_read_camera", "use_odom_shift", false);
  agv_navigation_type_ = SharedParamsInterface::getParam<int>("AGVParam", "agv_navigation_type", 0);
  current_floor_ = SharedParamsInterface::getParam<std::string>("AGVParam", "current_floor", default_floor);

  if (first_init) {
    first_init = false;
    DEBUG_STREAM_OUT( "get param code_miss_num value:" << qr_code_miss_threshold_);
    DEBUG_STREAM_OUT( "get param code_miss_distance value:" << qr_code_miss_distance_);
    DEBUG_STREAM_OUT( "get param tag_count_num value:" << tag_count_num_);
    DEBUG_STREAM_OUT( "get param filter_switch value:" << filter_switch_);
    DEBUG_STREAM_OUT( "get param scale value:" << filter_scale_);
    DEBUG_STREAM_OUT( "get param use_odom_shift value:" << use_odom_shift_);
    DEBUG_STREAM_OUT( "get param agv_navigation_type value:" << agv_navigation_type_);
    DEBUG_STREAM_OUT( "get param current_floor value:" << current_floor_);
  }
}

bool Qrodom::init_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    DEBUG_ERROR_OUT("Failed to initialize shared memory interface");
    return false;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      DEBUG_ERROR_OUT("Error get shared parameters");
      return false;
    } 
    get_qrodom_paramters();
  } catch (const std::exception& e) {
    DEBUG_ERROR_OUT(
      "Error reading parameters: " << e.what());
    return false;
  }
  return true;
}

agv_msgs::msg::OdometryLite Qrodom::get_current_odometry()
{
  std::lock_guard<std::mutex> lock(mutexB_);
  return current_odometry_;
}

void Qrodom::set_current_odometry(const agv_msgs::msg::OdometryLite &msg)
{
  std::lock_guard<std::mutex> lock(mutexB_);
  current_odometry_ = msg;
}

double Qrodom::get_current_yaw()
{
  std::lock_guard<std::mutex> lock(mutexC_);
  return pose_yaw_;
}

void Qrodom::set_current_yaw(double &yaw)
{
  std::lock_guard<std::mutex> lock(mutexC_);
  pose_yaw_ = yaw;
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Qrodom>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

